

#include <stdint.h>

//#include <stdlib.h>
#include <stm32l475xx.h>

#include <core_cm4.h>
#define  RCC_CR       		    (*(volatile uint32_t *) 0x40021000)
#define  RCC_CFGR       		(*(volatile uint32_t *) 0x40021008)
#define  RCC_CIER      		    (*(volatile uint32_t *) 0x40021088)
#define  RCC_AHB2ENR      		 (*(volatile uint32_t *) 0x4002104C)

#define  GPIOE     				(*(volatile uint32_t *) 0x48001000)
#define  GPIOE_MODER			(*(volatile uint32_t *) 0x48001000)
#define  GPIOE_OTYPER			(*(volatile uint32_t *) 0x48001004)
#define  GPIOE_OSPEEDR			(*(volatile uint32_t *) 0x48001008)
#define  GPIOE_PUPDR			(*(volatile uint32_t *) 0x4800100C)
#define  GPIOE_ODR				(*(volatile uint32_t *) 0x48001014)


void SystemInit(void)
{
	
	RCC_CR  |=   1<<3;  //select msi range from RCC_CR[7:4]
	RCC_CR  |=   0x7 << 4;  //configure msi clk as 8mHz
	
	RCC_CFGR  &= 	~(0x3 << 0);   //select MSI as sys_clk source;
	
	RCC_CR |=  1<<0; //enable mis clk;
	while( (RCC_CR & 0x02) ==0);   //wait until  MSI clk enabled;
}



led_init(){
	//enable gpioe  clock
	RCC_AHB2ENR  |= 1<<4;

	//config  gpioe-7 as output 
	GPIOE_MODER = 0;
	GPIOE_MODER |=  (0x01 << 14)  | (0x01<<  (8<<2))  ;   //output
	GPIOE_MODER |=  (0x01 << 16)  ;   //output
	
	GPIOE_OTYPER  &=  ~(1<<7);    //push-pull
	GPIOE_OTYPER  &=  ~(1<<8);    //push-pull
	
	GPIOE_PUPDR &= ~(0x03 <<7);  //no pull up-down.
	GPIOE_PUPDR &= ~(0x03 <<8);  //no pull up-down.
}

void light_red_led(int state){
	
#if 0
	//enable gpioe  clock
	RCC_AHB2ENR  |= 1<<4;

	//config  gpioe-7 as output 
	GPIOE_MODER = 0;
	GPIOE_MODER |=  (0x01 << 14);   //output
	GPIOE_OTYPER  &=  ~(1<<7);   //push-pull
	GPIOE_PUPDR &= ~(0x03 <<7);  //no pull up-down.
#endif
	if(state)
		GPIOE_ODR   &= ~(1<<7); //light  Red led
	else
		GPIOE_ODR  |= 1<<7;
}


void light_green_led(int state){
	
	if(state)
		GPIOE_ODR   &= ~(1<<8); //light  green led
	else
		GPIOE_ODR  |= 1<<8;
}




void __libc_init_array(){}


void task0(){
	
   while(1){
	   light_red_led(1);
	   light_green_led(0);
   }
}

void task1(){
	
    while(1){
	   light_red_led(0);
	   light_green_led(1);
    }
}

uint32_t  task0_stack[1024];
uint32_t  task1_stack[1024];

uint32_t * psp_stacks[2];


int volatile running_task; 



void tasks_init(){

	
   uint32_t *p;
   psp_stacks[0] = ( uint32_t *) ( task0_stack + (sizeof task0_stack)  - 8*4);

   p =  psp_stacks[0]+7 ;
   *p = 0x01000000;

   p =  psp_stacks[0]+6 ;
   *p = (uint32_t) task1;
   
   p =  psp_stacks[0]+5 ;
   *p = (uint32_t) task1;




   psp_stacks[1] = (int32_t *) ( task1_stack + (sizeof task1_stack)  - 8*4);

   p =  psp_stacks[1]+7 ;
   *p = 0x01000000;

   p = psp_stacks[1]+6 ;
   *p =  (uint32_t) task1;
   
   p = psp_stacks[1]+5 ;
   *p =  (uint32_t) task1;
   
   
   p = psp_stacks[1] +3;
   *p =  0x03030303;
   
   p = psp_stacks[1] +2;
   *p =  0x02020202;
   
   p = psp_stacks[1] +1;
   *p =  0x01010101;
   
   p = psp_stacks[1] ;
   *p =  0x00000000;
   
    running_task = 0;
}




void os_init()
{
 
    SysTick_Config(8000*1000);   //1seconds 8000*1000
    NVIC_SetPriority(PendSV_IRQn, 0xFF);
    //SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
#if 0
    __asm  volatile (  
		"svc 0 \n" 
		"nop \n"
		);
#endif

    __set_PSP(psp_stacks[0] + 16);
	
	__set_CONTROL(0x3);
	
	__asm volatile (
        "isb;"
    );
	
	task0();
}



int main(){

    SystemInit();

    led_init();
    light_red_led(0);
    light_green_led(0);
    
	tasks_init();
    os_init();
    for(;;);
}

void SVC_Handler(){
  
}

void switch_task(uint32_t task_sp){
	
	if(running_task  == 0){
		psp_stacks[0] = task_sp;
		running_task = 1;
		__set_PSP(psp_stacks[running_task]);
	}else if(running_task  == 1){
		psp_stacks[1] = task_sp;
		running_task = 0;
		__set_PSP(psp_stacks[running_task]);
	}
}

volatile uint32_t psp;
volatile uint32_t msp;

void PendSV_Handler(){
  psp = __get_PSP();
  msp = __get_MSP();
  
  if(psp == 1 || msp == 1 )
       return ;
   

     __asm volatile (
        "mrs r0, psp;"
		"isb ;"
		"push  {r4-r11, lr};"
		"bl %c0;"		   //save new psp to current thread;
		"pop  {r4-r11,lr};"
		"mrs r0, psp;"     //get new task psp
		"ldr r1, =0xFFFFFFFD;"
		"bx  r1;"
		:: "i"(switch_task)
	);

}


void SysTick_Handler(){
  psp = __get_PSP();
  msp = __get_MSP();
    SCB->ICSR  |= SCB_ICSR_PENDSVSET_Msk;
  if(psp == 1 || msp == 1 )
       return ;
}
